package es.uned.dia.james;

public class LinearPlant {
	 double[] x, u;
	 double[][] A, B;
	 int states, inputs;
	 Delay[] delay;

	 // Initialize the Plant. Reserves memory for x, u, A and B
	 public void initialize(int states, int inputs) {
	     this.states = states;
		 this.inputs = inputs;
		 this.x = new double[states]; 
		 this.u = new double[inputs];    
		 // Transition Matrix (NxN)
		 this.A = new double[states][states];
	    // Input Matrix (NxM)
		 this.B = new double[states][inputs];
		 // Input Delay
		 this.delay = new Delay[inputs];
		 for(int i=0; i<inputs; i++) delay[i] = new Delay(0.0);
	 }
	 
	  // Constructor
	 public LinearPlant(int states, int inputs) {  	      
	     initialize(states, inputs);
	 }
	  
	  // Constructor
	 public LinearPlant(double[][] A, double[][] B) {		  
	     int M=A.length, N=A[0].length, P=B.length, Q=B[0].length;  
	     if(M!=N || N!=P) return;
	     initialize(N, Q);
	     setA(A); setB(B);
	 }

	  // The derivatives of the states variables
	 public double f(double[] x, double[] u, int i) {
	     double dx = 0;

	     for(int j=0; j<states; j++) dx += A[i][j]*x[j];	    
	     for(int j=0; j<inputs; j++) dx += B[i][j]*u[j];
	     
	     return dx;
	  }

	 public void setA(double[][] A) {
	     int M=this.A.length, N=this.A[0].length, P=A.length, Q=A[0].length;
	     if(M!=P || N!=Q) return;

	     for(int i=0; i<M; i++) for(int j=0; j<N; j++) this.A[i][j] = A[i][j];

	     return;
	  }
	  
	 public void setB(double[][] B) {
	     int M=this.B.length, N=this.B[0].length, P=B.length, Q=B[0].length;
	     if(M!=P || N!=Q || M!=states || Q!=inputs) return;

	     for(int i=0; i<M; i++) for(int j=0; j<N; j++) this.B[i][j] = B[i][j];	   
	     return;
	 }

	 public boolean setInputDelay(int index, double L) {	     
	     if(index < 0 || index >= inputs) return false;
	     
	     delay[index] = new Delay(L); 
	     return false;
	 }
	 
	 public void applyInput(double t, double[] u) {
	     for(int i=0; i<inputs; i++) delay[i].add(t, u[i]);
	 }
	 
	//  public void setC(double[][] C) {copyMatrix(this.C, C);}
	//  public void setD(double[][] D) {copyMatrix(this.D, D);}
}
